#!/usr/bin/env python
# BEGIN ALL
# coding=utf-8
import rospy, cv2, numpy
from sensor_msgs.msg import Image
import sys
import cv_bridge
import pick_up_item
import put_item

class Catch:
  def __init__(self):
    self.bridge = cv_bridge.CvBridge()
    self.image_sub = rospy.Subscriber('head_camera/rgb/image_raw', 
                                      Image, self.image_callback)
    self.n = 0
  def image_callback(self, msg):
    image = self.bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8')
    img_hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
    # Begin to catch when objects arrive at the position
    print("-"*20)
    print(numpy.mean(img_hsv[172:229, 181:201, 0]), self.n)
    if numpy.mean(img_hsv[172:229, 181:201, 0])<99 and self.n <= 1:
        self.n += 1
        if self.n == 2:
            pick_up_item.main()
            put_item.main()
    print("-"*20)
    # display
    cv2.imshow("hsv", img_hsv)
    cv2.waitKey(2)


rospy.init_node('Catch')
catchbin = Catch()
rospy.spin()
# END ALL


